#pragma once
#include <iostream>
#include <queue>
#include <vector>
#include "Point.hpp"

class Agv
{
private:
    Point position_;
    std::queue<Point> path_; // 记录路径
    bool status_;

public:
    bool GetStatus()
    {
        return status_;
    }
    Agv(Point startPos = Point(0, 0), bool status = false) : position_(startPos), status_(status) {}

    void setPath(const std::vector<Point> &newPath)
    {
        while (!path_.empty())
        {
            path_.pop();
        }
        for (auto &p : newPath)
        {
            path_.push(p);
        }
    }

    void moveTo()
    {
        if (!path_.empty())
        {
            position_ = path_.front();
            path_.pop();
            std::cout << "移动到：" << position_.x << "," << position_.y << std::endl;
        }
        else
        {
            status_ = false;
            std::cout << "AGV已经到达了目标位置" << std::endl;
        }
    }

    Point getPosition() const
    {
        return position_;
    }

    std::queue<Point> getPath() const
    {
        return path_;
    }
};